package model

import (
	"encoding/json"
	"math"
	"sync"
	"time"

	"dev.ixmxm.top/rms/utils/angles"
	"dev.ixmxm.top/rms/utils/log"
)

type Robot struct {
	Account           *RobotAccount //机器人连接信息
	Type              RobotType     //机器人类型
	Status            *RobotStatus  //机器人状态
	statusMutex       sync.Mutex    //机器人状态更新的锁，写的时候锁，读取没有必要锁
	allocateLocker    sync.Mutex    //是否分配的锁
	Allocated         bool          //本轮是否已经分配
	AgvCorverNodes    []string
	AgvCorverPaths    []string
	CanBeCancel       bool //任务可打断标志位
	TaskOnline        bool //机器人是否调用上线接口,用于解决可能小车没有调用TaskOnline接口会下发任务
	lastReconnectTime time.Time
	LastSiteId        string
	LocalPathNum      int
}
type DealRobot struct {
	Robots                   []*Robot
	DealGroundLocationRobots []*Robot
}

// AllocateLock 锁住机器人分配
func (r *Robot) AllocateLock() {
	r.allocateLocker.Lock()
}

// AllocateUnLock 释放机器人分配锁
func (r *Robot) AllocateUnLock() {
	r.allocateLocker.Unlock()
}

func (r *Robot) UpdateStatus(status *RobotStatus) {
	r.statusMutex.Lock()
	defer r.statusMutex.Unlock()
	status.Online = true
	status.TaskOnline = r.TaskOnline
	//给机器人赋值
	var outLine OutLine
	var isBlock bool
	if r.Status != nil {
		outLine = r.Status.CarOutline
		isBlock = r.Status.IsBlock
		if outLine.RealHeadLength == 0 || outLine.RealTailLength == 0 || outLine.RealHalfWidth == 0 {
			outLine.RealHeadLength = float64(r.Type.LengthToFront)
			outLine.RealTailLength = float64(r.Type.LengthToBack)
			outLine.RealHalfWidth = float64(r.Type.Width / 2)
		}
	}

	// status.GoodsStatus = 1
	r.Status = status
	r.Status.LowPowerThreshold = r.Type.ChargePercent
	r.Status.CarOutline = outLine
	r.Status.IsBlock = isBlock
}

func (r *Robot) UpdateTaskOnlineStatus(taskOnline bool) {
	r.statusMutex.Lock()
	defer r.statusMutex.Unlock()
	r.TaskOnline = taskOnline
	if r.Status != nil {
		r.Status.TaskOnline = taskOnline
	}
}
func (r *Robot) UpdateTaskFlg(canBeCancel bool) {
	r.statusMutex.Lock()
	defer r.statusMutex.Unlock()
	r.CanBeCancel = canBeCancel
}

func (r *Robot) GetStatus() RobotStatus {
	// r.statusMutex.RLock()
	// defer r.statusMutex.RUnlock()
	s := *r.Status
	return s
}

//小车是否存在状态信息
func (r *Robot) StatusIsExists() bool {
	if r == nil || r.Account == nil || !r.Account.IsEnable || r.Status == nil || r.Status.SiteId < 0 {
		return false
	}
	return true
}

//检测机器人是否可用
func (r *Robot) RobotIsUse() bool {
	if !r.StatusIsExists() {
		//log.Warn("robot account is not enable:", r.Account.AgvId)
		return false
	}
	if !r.Status.Online {
		//log.Warn("robot is not online:", r.Account.AgvId)
		return false
	}
	if !r.Status.TaskOnline {
		//log.Warn("robot is not online:", r.Account.AgvId)
		return false
	}
	if r.Status.EmergencyStop == 1 {
		//log.Warn("robot is emergency stop:", r.Account.AgvId)
		return false
	}
	if r.Status.ControlType == 0 {
		//log.Warn("robot's control type is manual:", r.Account.AgvId)
		return false
	}
	if r.Status.SoftStop == 1 {
		return false
	}

	if !r.Account.IsEnable {
		//log.Warnf("robot %s is not enable", r.Account.AgvId)
		return false
	}

	status := r.Status.Status
	if status == RobotStatusCharging && r.Status.Power <= r.Type.ChargeToTaskPercent {
		//log.Warn("robot is charging and battery low:", r.Account.AgvId)
		return false
	}
	return true
}
func (r *Robot) GetDeepCopyRobot() *Robot {
	var robotDeepCopy Robot
	temp, _ := json.Marshal(r)
	json.Unmarshal(temp, &robotDeepCopy)
	return &robotDeepCopy
}
func (r *Robot) GetNextTargetSiteIdDeepCopyRobot(targetSiteId string, targetCarBody TargetCarPose) *Robot {
	//深拷贝
	var robotCopy Robot
	temp, _ := json.Marshal(r)
	json.Unmarshal(temp, &robotCopy)
	robotCopy.Status.Angle = int(targetCarBody.CarBodyRad * 180 / math.Pi)
	robotCopy.Status.SiteIdS = targetCarBody.TargetSiteId
	return &robotCopy
}

//
func (robotType RobotType) PathConflictRect(robot *Robot, path Path, theta, x, y float64, isMaxRange bool) bool {
	pathParam := robotType.GetRobotPathParam(robot, isMaxRange)
	lockHeadRange2 := pathParam.lockHeadRange2
	lockTailRange2 := pathParam.lockTailRange2
	lockWidthRange2 := 2 * pathParam.lockWidthRange2
	var P1, P2, P3, P4 Pose2
	P1.X, P1.Y = x+lockHeadRange2*math.Cos(theta)-lockWidthRange2*math.Sin(theta), y+lockHeadRange2*math.Sin(theta)+lockWidthRange2*math.Cos(theta)
	P2.X, P2.Y = x+(-lockTailRange2*math.Cos(theta)-lockWidthRange2*math.Sin(theta)), y+(-lockTailRange2*math.Sin(theta)+lockWidthRange2*math.Cos(theta))
	P3.X, P3.Y = x+(-lockTailRange2*math.Cos(theta)+lockWidthRange2*math.Sin(theta)), y+(-lockTailRange2*math.Sin(theta)-lockWidthRange2*math.Cos(theta))
	P4.X, P4.Y = x+lockHeadRange2*math.Cos(theta)+lockWidthRange2*math.Sin(theta), y+lockHeadRange2*math.Sin(theta)-lockWidthRange2*math.Cos(theta)
	inRectangle := func(x, y float64) bool {

		getCross := func(p1, p2 Pose2) float64 {
			return (p2.X-p1.X)*(y-p1.Y) - (x-p1.X)*(p2.Y-p1.Y)
		}
		return getCross(P3, P2)*getCross(P1, P4) >= 0 &&
			getCross(P2, P1)*getCross(P4, P3) >= 0
	}
	blockRect := func(path Path) bool {
		center_x := (P1.X + P2.X + P3.X + P4.X) / 4
		center_y := (P1.Y + P2.Y + P3.Y + P4.Y) / 4
		xa, ya, xb, yb := float64(path.NodeA.X), float64(path.NodeA.Y), float64(path.NodeB.X), float64(path.NodeB.Y)
		nearest_x, nearest_y := 0.0, 0.0
		if (xb-xa)*(center_x-xa)+(yb-ya)*(center_y-ya) < 0 {
			nearest_x, nearest_y = xa, ya
		} else if (xa-xb)*(center_x-xb)+(ya-yb)*(center_y-yb) < 0 {
			nearest_x, nearest_y = xb, yb
		} else {
			if xa == xb {
				nearest_x, nearest_y = xa, center_y
			} else if ya == yb {
				nearest_x, nearest_y = center_x, ya
			} else {
				k := (yb - ya) / (xb - xa)
				b := yb - k*xb
				//两条垂直相交直线的斜率相乘积为-1：k1*k2=-1.
				// nearest_y=k*nearest_x+b 与 (center_y-nearest_y)/(center_x-nearest_x) = -1/k，已知center_x,center_y,求nearest_x,nearest_y
				nearest_x = (k*center_y + center_x - k*b) / (k*k + 1)
				nearest_y = k*nearest_x + b
			}
		}
		return inRectangle(nearest_x, nearest_y)
	}
	if blockRect(path) {
		return true
	}
	return false
}

//计算填充旋转参数，尾部长度
func (robotType *RobotType) SetRobotTypeParams() {
	robotType.LengthToBack = float64(robotType.Length - robotType.LengthToFront)
	robotType.GoodsLengthToBack = float64(robotType.Goodslength - robotType.LengthToFront)
	robotType.RotateRadius = math.Sqrt(math.Pow(float64(robotType.LengthToFront), 2) + math.Pow(float64(robotType.Width/2), 2))
	robotType.RotateBackRadius = math.Sqrt(math.Pow(robotType.LengthToBack, 2) + math.Pow(float64(robotType.Width/2), 2))
	robotType.GoodsRotateRadius = math.Sqrt(math.Pow(float64(robotType.LengthToFront), 2) + math.Pow(float64(robotType.GoodsWidth)/2, 2))
	robotType.GoodsRotateBackRadius = math.Sqrt(math.Pow(robotType.GoodsLengthToBack, 2) + math.Pow(float64(robotType.GoodsWidth/2), 2))
}

//根据机器人获得前端旋转半径
func (robotType RobotType) GetRobotRotateRadius(robot *Robot) float64 {
	R := float64(robotType.RotateRadius) + float64(robotType.RotateSafeDis)
	if robot != nil && robot.Account != nil {
		//状态为载货状态
		if robot.Account.GoodsStatus != 0 {
			R = float64(robotType.GoodsRotateRadius) + float64(robotType.RotateSafeDis)
		}
	}
	return R
}

//根据机器人获得后端旋转半径
func (robotType RobotType) GetRobotBackRotateRadius(robot *Robot) float64 {
	r := float64(robotType.RotateBackRadius) + float64(robotType.BackSafeDis)
	if robot != nil && robot.Account != nil {
		//状态为载货状态
		if robot.Account.GoodsStatus != 0 {
			r = float64(robotType.GoodsRotateBackRadius) + float64(robotType.BackSafeDis)
		}
	}
	return r
}

//根据机器人获得宽
func (robotType RobotType) GetRobotHalfWidth(robot *Robot) float64 {
	W := float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)
	if robot != nil && robot.Account != nil {
		//状态为载货状态
		if robot.Account.GoodsStatus != 0 {
			W = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)
		}
	}
	return W
}

func (robotType RobotType) GetRobotPathParam(robot *Robot, isMaxRange bool) struct {
	lockHeadRange2  float64
	lockTailRange2  float64
	lockWidthRange2 float64
} {
	carPathParam := struct {
		lockHeadRange2  float64
		lockTailRange2  float64
		lockWidthRange2 float64
	}{}
	//根据状态
	//1.正常无货情况下
	carPathParam.lockTailRange2 = float64(robotType.LengthToBack) + float64(robotType.BackSafeDis)
	carPathParam.lockWidthRange2 = float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)
	carPathParam.lockHeadRange2 = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	if isMaxRange {
		carPathParam.lockHeadRange2 = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.Width/2)
	}
	if robot.StatusIsExists() && robot.Account.GoodsStatus != 0 {
		//带货状态
		if isMaxRange {
			carPathParam.lockHeadRange2 = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.GoodsWidth/2)
		}
		carPathParam.lockTailRange2 = float64(robotType.GoodsLengthToBack) + float64(robotType.BackSafeDis)
		carPathParam.lockWidthRange2 = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)

	}
	return carPathParam

}

//根据机器人类型赋值新的参数
func (robotType RobotType) GetRealRobotBodyParam(robot *Robot, target_type NodeType, isFresh, isMaxHead bool) struct {
	lHead float64
	lTail float64
	W     float64
	R     float64
	r     float64
} {
	carBodyParam := struct {
		lHead float64
		lTail float64
		W     float64
		R     float64
		r     float64
	}{}

	//-----------------加上安全轮廓后的值---------------------
	//1.正常无货情况下
	carBodyParam.lHead = float64(robotType.LengthToFront) + float64(robotType.SafeDis)
	carBodyParam.lTail = float64(robotType.LengthToBack) + float64(robotType.BackSafeDis)
	carBodyParam.W = float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)
	carBodyParam.R = float64(robotType.RotateRadius) + float64(robotType.RotateSafeDis)
	carBodyParam.r = float64(robotType.RotateBackRadius) + float64(robotType.BackSafeDis)
	//根据相关处理操作处理
	// if target_type == NodeStation || target_type == NodeWaiting || target_type == NodeAvoid || target_type == NodeCharge {
	// 	if robot.StatusIsExists() {
	// 		carBodyParam.lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	// 	}
	// }
	if target_type == NodeAvoid || target_type == NodeCharge {
		carBodyParam.lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	} else if target_type == NodeStation || target_type == NodeWaiting {
		if isMaxHead {
			carBodyParam.lHead = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.Width/2)
		} else {
			carBodyParam.lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
		}
	}

	//给AGV状态赋值
	//传参数
	realWidth := robotType.Width
	realTailLength := robotType.LengthToBack
	if robot.StatusIsExists() {
		//带货状态
		if robot.Account.GoodsStatus != 0 {
			//
			realWidth = robotType.GoodsWidth
			realTailLength = robotType.GoodsLengthToBack
			carBodyParam.lTail = float64(robotType.GoodsLengthToBack) + float64(robotType.BackSafeDis)
			carBodyParam.W = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)
			carBodyParam.R = float64(robotType.GoodsRotateRadius) + float64(robotType.RotateSafeDis)
			carBodyParam.r = float64(robotType.GoodsRotateBackRadius) + float64(robotType.BackSafeDis)

			if target_type == NodeStation || target_type == NodeWaiting {
				if isMaxHead {
					carBodyParam.lHead = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + 0.5*float64(robotType.GoodsWidth)
				} else {
					carBodyParam.lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
				}
			}
		}
		if isFresh {
			robot.Status.CarOutline.RealHeadLength = float64(robotType.LengthToFront)
			robot.Status.CarOutline.RealHalfWidth = float64(realWidth) / 2
			robot.Status.CarOutline.RealTailLength = realTailLength
			//外部轮廓
			robot.Status.CarOutline.HeadLength = carBodyParam.lHead
			robot.Status.CarOutline.TailLength = carBodyParam.lTail
			robot.Status.CarOutline.HalfWidth = carBodyParam.W
			robot.Status.CarOutline.FrontRadius = carBodyParam.R
			robot.Status.CarOutline.BackRadius = carBodyParam.r
		}
	}
	return carBodyParam
}

func (robotType RobotType) RefreshRobotStatus(robot *Robot, target_type NodeType, isUseMaxHead bool) {
	//1.正常无货情况下
	lHead := float64(robotType.LengthToFront) + float64(robotType.SafeDis)
	lTail := float64(robotType.LengthToBack) + float64(robotType.BackSafeDis)
	W := float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)
	R := float64(robotType.RotateRadius) + float64(robotType.RotateSafeDis)
	r := float64(robotType.RotateBackRadius) + float64(robotType.BackSafeDis)

	if target_type == NodeAvoid || target_type == NodeCharge {
		lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	} else if target_type == NodeStation || target_type == NodeWaiting {
		if isUseMaxHead {
			lHead = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.Width/2)
		} else {
			lHead = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
		}
	}

	//存在robot,并且有货，根据robot轮廓计算
	realWidth := robotType.Width
	realTailLength := robotType.LengthToBack
	if robot.StatusIsExists() {
		if robot.Account.GoodsStatus != 0 {
			realWidth = robotType.GoodsWidth
			realTailLength = robotType.GoodsLengthToBack
			//判断轮廓
			lTail = float64(robotType.GoodsLengthToBack) + float64(robotType.BackSafeDis)
			W = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)
			R = float64(robotType.GoodsRotateRadius) + float64(robotType.RotateSafeDis)
			r = float64(robotType.GoodsRotateBackRadius) + float64(robotType.BackSafeDis)
			if target_type == NodeStation || target_type == NodeWaiting {
				if isUseMaxHead {
					lHead = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.GoodsWidth/2)
				}
			}
		}
		robot.Status.CarOutline.RealHeadLength = float64(robotType.LengthToFront)
		robot.Status.CarOutline.RealHalfWidth = float64(realWidth) / 2
		robot.Status.CarOutline.RealTailLength = realTailLength
		//外部轮廓
		robot.Status.CarOutline.HeadLength = lHead
		robot.Status.CarOutline.TailLength = lTail
		robot.Status.CarOutline.HalfWidth = W
		robot.Status.CarOutline.FrontRadius = R
		robot.Status.CarOutline.BackRadius = r
	}
}

// 上线和任务结束的情况下，计算应该锁的车体大小
func (robotType RobotType) IsLockHeadMaxRectangle(node Node, robot *Robot, theta, x, y, path_length float64, target_type NodeType) bool {
	//判断点的类型是否属于特殊点
	if robot == nil || robot.Account == nil {
		return false
	}
	if target_type != NodeStation && target_type != NodeWaiting {
		return false
	}

	lockHeadMaxRange := float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.Width/2)
	lockHeadMinRange := float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	lockHeadHalfWidthRange := float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)

	if robot.Account.GoodsStatus != 0 {
		lockHeadMaxRange = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.GoodsWidth/2)
		lockHeadHalfWidthRange = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)
	}

	inRectangle1 := func(node Node, theta, x, y, lHeadMax, lHeadMin, Width float64) bool {
		x1, y1 := x+lHeadMax*math.Cos(theta)-Width*math.Sin(theta), y+lHeadMax*math.Sin(theta)+Width*math.Cos(theta)
		x2, y2 := x+lHeadMin*math.Cos(theta)-Width*math.Sin(theta), y+lHeadMin*math.Sin(theta)+Width*math.Cos(theta)
		x3, y3 := x+lHeadMin*math.Cos(theta)+Width*math.Sin(theta), y+lHeadMin*math.Sin(theta)-Width*math.Cos(theta)
		x4, y4 := x+lHeadMax*math.Cos(theta)+Width*math.Sin(theta), y+lHeadMax*math.Sin(theta)-Width*math.Cos(theta)
		getCross := func(p1x, p1y, p2x, p2y float64) float64 {
			return (p2x-p1x)*(float64(node.Y)-p1y) - (float64(node.X)-p1x)*(p2y-p1y)
		}
		return getCross(x3, y3, x2, y2)*getCross(x1, y1, x4, y4) >= 0 && getCross(x2, y2, x1, y1)*getCross(x4, y4, x3, y3) >= 0
	}
	return inRectangle1(node, theta, x, y, lockHeadMaxRange, lockHeadMinRange, lockHeadHalfWidthRange)

}

// x,y,theta 坐标
func (robotType RobotType) InLockRectangle(node Node, robot *Robot, theta, x, y, path_length float64, target_type NodeType, isMaxHead bool) bool {
	lockHeadRange := float64(robotType.LengthToFront) + float64(robotType.SafeDis)
	lockTailRange := float64(robotType.LengthToBack) + 0.5*float64(robotType.BackSafeDis)
	lockWidthRange := float64(robotType.Width/2) + 0.5*float64(robotType.SideSafeDis)

	if target_type == NodeAvoid || target_type == NodeCharge {
		lockHeadRange = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
	} else if target_type == NodeStation || target_type == NodeWaiting {
		if isMaxHead {
			lockHeadRange = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.Width/2)
		} else {
			lockHeadRange = float64(robotType.LengthToFront) + 0.5*float64(robotType.SideSafeDis)
		}
	}

	//存在robot,并且有货，根据robot轮廓计算
	if robot != nil && robot.Status != nil && robot.Account.GoodsStatus != 0 {
		//判断轮廓
		lockHeadRange = float64(robotType.LengthToFront) + float64(robotType.SafeDis)
		lockTailRange = float64(robotType.GoodsLengthToBack) + 0.5*float64(robotType.BackSafeDis)
		lockWidthRange = float64(robotType.GoodsWidth/2) + 0.5*float64(robotType.SideSafeDis)
		if target_type == NodeStation || target_type == NodeWaiting {
			if isMaxHead {
				lockHeadRange = float64(robotType.LengthToFront) + float64(robotType.SideSafeDis) + float64(robotType.GoodsWidth/2)
			}
		}
	}
	lockHeadRange += 0.5 * path_length
	lockTailRange += 0.5 * path_length

	inRectangle := func(node Node, theta, x, y, lHead, lTail, Width float64) bool {
		x1, y1 := x+lHead*math.Cos(theta)-Width*math.Sin(theta), y+lHead*math.Sin(theta)+Width*math.Cos(theta)
		x2, y2 := x+(-lTail*math.Cos(theta)-Width*math.Sin(theta)), y+(-lTail*math.Sin(theta)+Width*math.Cos(theta))
		x3, y3 := x+(-lTail*math.Cos(theta)+Width*math.Sin(theta)), y+(-lTail*math.Sin(theta)-Width*math.Cos(theta))
		x4, y4 := x+lHead*math.Cos(theta)+Width*math.Sin(theta), y+lHead*math.Sin(theta)-Width*math.Cos(theta)
		getCross := func(p1x, p1y, p2x, p2y float64) float64 {
			return (p2x-p1x)*(float64(node.Y)-p1y) - (float64(node.X)-p1x)*(p2y-p1y)
		}
		return getCross(x3, y3, x2, y2)*getCross(x1, y1, x4, y4) >= 0 && getCross(x2, y2, x1, y1)*getCross(x4, y4, x3, y3) >= 0
	}
	return inRectangle(node, theta, x, y, lockHeadRange, lockTailRange, lockWidthRange)
}

func (robotType RobotType) ComputeBezierPolygon(p *TrafficPath, robot *Robot) []Pose2 {
	width, radius_r, radius_R := robotType.GetRobotHalfWidth(robot)*2, robotType.GetRobotBackRotateRadius(robot), float64(robotType.LengthToFront)+float64(robotType.SafeDis)
	//P点在坐标系X'OY'中的坐标（x',y'）,可以求得点P在基坐标系XOY中的坐标值
	//x=x'·cos(-θ)+y'·sin(-θ)
	//y=y'·cos(-θ)-x'·sin(-θ)
	computeNewPose := func(point Pose2, theta, xn, yn float64) Pose2 {
		var pose Pose2
		pose.X = point.X + xn*math.Cos(-theta) + yn*math.Sin(-theta)
		pose.Y = point.Y + yn*math.Cos(-theta) - xn*math.Sin(-theta)
		return pose
	}
	calculateBezierPointForCubic := func(t float64, p0, p1, p2, p3 Pose2) Pose2 {
		temp := 1 - t
		var point Pose2
		point.X = p0.X*temp*temp*temp + 3*p1.X*t*temp*temp + 3*p2.X*t*t*temp + p3.X*t*t*t
		point.Y = p0.Y*temp*temp*temp + 3*p1.Y*t*temp*temp + 3*p2.Y*t*t*temp + p3.Y*t*t*t
		return point
	}
	poseList := make([]Pose2, 0)
	var pointS = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
	var pointSc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
	var pointTc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
	var pointT = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
	if p.BodyDirec == HeadBA || (p.BodyDirec == Head && p.Dir == DirectionB2A) {
		pointS = Pose2{X: float64(p.NodeB.X), Y: float64(p.NodeB.Y)}
		pointSc = Pose2{X: p.NodeBControlX, Y: p.NodeBControlY}
		pointTc = Pose2{X: p.NodeAControlX, Y: p.NodeAControlY}
		pointT = Pose2{X: float64(p.NodeA.X), Y: float64(p.NodeA.Y)}
	}
	pointM := calculateBezierPointForCubic(0.5, pointS, pointSc, pointTc, pointT)
	pointM1 := calculateBezierPointForCubic(0.51, pointS, pointSc, pointTc, pointT)
	pointSM := calculateBezierPointForCubic(0.25, pointS, pointSc, pointTc, pointT)
	pointSM1 := calculateBezierPointForCubic(0.26, pointS, pointSc, pointTc, pointT)
	pointMT := calculateBezierPointForCubic(0.75, pointS, pointSc, pointTc, pointT)
	pointMT1 := calculateBezierPointForCubic(0.76, pointS, pointSc, pointTc, pointT)
	angleS := math.Atan2(pointSc.Y-pointS.Y, pointSc.X-pointS.X)
	angleT := math.Atan2(pointT.Y-pointTc.Y, pointT.X-pointTc.X)
	angleM := math.Atan2(pointM1.Y-pointM.Y, pointM1.X-pointM.X)
	angleSM := math.Atan2(pointSM1.Y-pointSM.Y, pointSM1.X-pointSM.X)
	angleMT := math.Atan2(pointMT1.Y-pointMT.Y, pointMT1.X-pointMT.X)

	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, width/2))
	if angles.NormalizeAngle(angleM-angleS) > 0.0 {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, 0, width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointS, angleS, radius_R, width/2))
		poseList = append(poseList, computeNewPose(pointSM, angleSM, radius_R, width/2))
	}
	if angles.NormalizeAngle(angleT-angleM) > 0.0 {
		if angles.NormalizeAngle(angleM-angleS) > 0.0 {
			poseList = append(poseList, computeNewPose(pointM, angleM, 0, width/2))
		}
		poseList = append(poseList, computeNewPose(pointT, angleT, 0, width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointM, angleM, radius_R, width/2))
		poseList = append(poseList, computeNewPose(pointMT, angleMT, radius_R, width/2))
	}
	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, width/2))
	poseList = append(poseList, computeNewPose(pointT, angleT, radius_R, -width/2))
	if angles.NormalizeAngle(angleT-angleM) > 0.0 {
		poseList = append(poseList, computeNewPose(pointMT, angleMT, radius_R, -width/2))
		poseList = append(poseList, computeNewPose(pointM, angleM, radius_R, -width/2))

	} else {
		poseList = append(poseList, computeNewPose(pointT, angleT, 0, -width/2))
		if angles.NormalizeAngle(angleM-angleS) < 0.0 {
			poseList = append(poseList, computeNewPose(pointM, angleM, 0, -width/2))
		}
	}
	if angles.NormalizeAngle(angleM-angleS) > 0.0 {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, radius_R, -width/2))
		poseList = append(poseList, computeNewPose(pointS, angleS, radius_R, -width/2))
	} else {
		poseList = append(poseList, computeNewPose(pointSM, angleSM, 0, -width/2))
	}
	poseList = append(poseList, computeNewPose(pointS, angleS, -radius_r, -width/2))
	return poseList
}

func (robotType RobotType) GotLockRectangle(robot *Robot, theta, x, y, path_length float64, target_type NodeType, isFresh, isMaxHead bool) []Pose2 {

	carBodyParam := robotType.GetRealRobotBodyParam(robot, target_type, isFresh, isMaxHead)
	carBodyParam.lHead += 0.5 * path_length
	carBodyParam.lTail += 0.5 * path_length
	var node_rect []Pose2
	node_rect = append(node_rect, Pose2{X: x + carBodyParam.lHead*math.Cos(theta) - carBodyParam.W*math.Sin(theta), Y: y + carBodyParam.lHead*math.Sin(theta) + carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x - carBodyParam.lTail*math.Cos(theta) - carBodyParam.W*math.Sin(theta), Y: y - carBodyParam.lTail*math.Sin(theta) + carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x - carBodyParam.lTail*math.Cos(theta) + carBodyParam.W*math.Sin(theta), Y: y - carBodyParam.lTail*math.Sin(theta) - carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x + carBodyParam.lHead*math.Cos(theta) + carBodyParam.W*math.Sin(theta), Y: y + carBodyParam.lHead*math.Sin(theta) - carBodyParam.W*math.Cos(theta)})
	return node_rect
}
func (robotType RobotType) GotRobotPolygonRangeXY(robot *Robot, theta, x, y float64) (minX float64, maxX float64, minY float64, maxY float64) {

	carBodyParam := robotType.GetRealRobotBodyParam(robot, NodeStation, false, true)
	var node_rect []Pose2
	node_rect = append(node_rect, Pose2{X: x + carBodyParam.lHead*math.Cos(theta) - carBodyParam.W*math.Sin(theta), Y: y + carBodyParam.lHead*math.Sin(theta) + carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x - carBodyParam.lTail*math.Cos(theta) - carBodyParam.W*math.Sin(theta), Y: y - carBodyParam.lTail*math.Sin(theta) + carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x - carBodyParam.lTail*math.Cos(theta) + carBodyParam.W*math.Sin(theta), Y: y - carBodyParam.lTail*math.Sin(theta) - carBodyParam.W*math.Cos(theta)})
	node_rect = append(node_rect, Pose2{X: x + carBodyParam.lHead*math.Cos(theta) + carBodyParam.W*math.Sin(theta), Y: y + carBodyParam.lHead*math.Sin(theta) - carBodyParam.W*math.Cos(theta)})
	minX = node_rect[0].X
	maxX = node_rect[0].X
	minY = node_rect[0].Y
	maxY = node_rect[0].Y
	for _, p := range node_rect {
		if p.X < minX {
			minX = p.X
		}

		if p.X > maxX {
			maxX = p.X
		}

		if p.Y < minY {
			minY = p.Y
		}

		if p.Y > maxY {
			maxY = p.Y
		}
	}
	return minX, maxX, minY, maxY
}

type RobotStatusCode uint8

const (
	RobotStatusUnknown       RobotStatusCode = 0  //未知
	RobotStatusIdle          RobotStatusCode = 1  //空闲
	RobotStatusInit          RobotStatusCode = 2  //初始化（小车软件刚启动并且小车不在站点上）
	RobotStatusEmergenceStop RobotStatusCode = 3  //急停（调度发送急停等待）
	RobotStatusError         RobotStatusCode = 4  //故障
	RobotStatusNavigating    RobotStatusCode = 5  //导航中
	RobotStatusPicking       RobotStatusCode = 6  //取货中
	RobotStatusPutting       RobotStatusCode = 8  //放货中
	RobotStatusCharging      RobotStatusCode = 10 //充电中
	RobotStatusExecStandby   RobotStatusCode = 12 //执行待命
	RobotStatusStandby       RobotStatusCode = 13 //待命中
	RobotStatusRunning       RobotStatusCode = 18 //执行中
)

func (code RobotStatusCode) In(status ...RobotStatusCode) bool {
	for _, s := range status {
		if code == s {
			return true
		}
	}
	return false
}

type RobotStatus struct {
	AgvId             string            `json:"agv_id" bson:"agv_id"`                   //小车ID
	CategoryId        int               `json:"category_id" bson:"category_id"`         //小车类型编号ID
	MapName           string            `json:"map_name" bson:"map_name"`               //地图名称
	CategoryType      RobotCategoryType `json:"category_type" bson:"category_type"`     //小车类型类别：0=堆垛车（熊猫车）、1=平衡重、2=前移车、3=三项叉、4=KIVA、5=暂存车、6=托盘车、7=侧叉车、8=全向车
	TaskId            uint64            `json:"task_id" bson:"task_id"`                 //当前执行子任务ID
	Power             int               `json:"power" bson:"power"`                     //电量(%)
	Angle             int               `json:"angle" bson:"angle"`                     //角度
	SiteId            int               `json:"-" bson:"site_id_int"`                   //点位ID
	SiteIdS           string            `json:"site_id" bson:"site_id"`                 //点位ID
	Speed             int               `json:"speed" bson:"speed"`                     //速度
	Status            RobotStatusCode   `json:"status" bson:"status"`                   //小车状态
	X                 int               `json:"x" bson:"x"`                             //X坐标
	Y                 int               `json:"y" bson:"y"`                             //Y坐标
	ControlType       int               `json:"control_type" bson:"control_type"`       //控制模式：0=手动、1=自动
	GoodsStatus       int32             `json:"goods_status" bson:"goods_status"`       //货物状态：0=无货、1=有货
	EmergencyStop     int32             `json:"emergency_stop" bson:"emergency_stop"`   //急停：0=正常、1=急停
	Online            bool              `json:"online" bson:"online"`                   //是否在线
	TaskOnline        bool              `json:"task_online" bson:"task_online"`         //是否任务在线;机器人是否调用上线接口,用于解决可能小车没有调用TaskOnline接口会下发任务
	ErrCode           string            `json:"err_code" bson:"err_code"`               //错误码
	AgvIP             string            `json:"agv_ip" bson:"agv_ip"`                   //ip地址
	AgvPort           int               `json:"agv_port" bson:"agv_port"`               //端口号
	AdditiveInfo      string            `json:"additive_info" bson:"additive_info"`     //附加信息
	ForkDirection     int32             `json:"fork_direction" bson:"fork_direction"`   //货叉方向，目前仅用于三向叉
	WheelAngle        int32             `json:"wheel_angle" bson:"wheel_angle"`         //舵轮角度
	ForkTaskIndex     int32             `json:"fork_task_index" bson:"fork_task_index"` //货叉当前动作阶段
	SoftStop          uint32            `json:"soft_stop" bson:"soft_stop"`             //软件停车：0=未停、1=已停
	LastCommTime      DateTime          `json:"last_comm_time" bson:"last_comm_time"`
	CarOutline        OutLine           `json:"car_outline" bson:"car_outline"`                 //车体动态轮廓
	IsBlock           bool              `json:"is_block" bson:"is_block"`                       //是否碰撞
	LowPowerThreshold int               `json:"low_power_threshold" bson:"low_power_threshold"` //低电量阈值
}

func (st *RobotStatus) ToString() string {
	data, _ := json.Marshal(st)
	return string(data)
}

type OutLine struct {
	FrontRadius float64 `json:"front_radius" bson:"front_radius"` //头部旋转半径
	BackRadius  float64 `json:"back_radius" bson:"back_radius"`   //尾部旋转半径

	HalfWidth  float64 `json:"half_width" bson:"half_width"`   //带安全距离半宽
	HeadLength float64 `json:"head_length" bson:"head_length"` //带安全距离头长
	TailLength float64 `json:"tail_length" bson:"tail_length"` //带安全距离尾长

	RealHalfWidth  float64 `json:"real_half_width" bson:"real_half_width"`   //真实半宽
	RealHeadLength float64 `json:"real_head_length" bson:"real_head_length"` //真实头长
	RealTailLength float64 `json:"real_tail_length" bson:"real_tail_length"` //真实尾长
}

//机器人是否在线状态更新
func (robot *Robot) Update(ip string, port int, reconnectCallback func(status *Robot)) {
	if robot.Status == nil {
		return
	}
	if reconnectCallback == nil {
		return
	}
	robot.Status.AgvIP = ip
	robot.Status.AgvPort = port
	robot.Status.Online = (time.Now().Unix() - robot.Status.LastCommTime.Unix()) < 3
	//超过10秒钟没有收到心跳包，机器上线改为失败
	if robot.Status.TaskOnline && time.Now().Unix()-robot.Status.LastCommTime.Unix() > 10 {
		log.Warnf("超过10秒钟没有收到心跳包，机器上线改为失败  robot:%s", robot.Status.AgvId)
		robot.TaskOnline = false
		robot.Status.TaskOnline = false
		reconnectCallback(robot)
	}
	//小车是手动状态，任务在线改为失败
	//达到一定次数后才
	if robot.Status.ControlType == 0 {
		// robot.UpdateTaskOnlineStatus(false)
		// log.Warnf("robot:%s 是手动状态，任务在线改成失败", robot.Status.AgvId)
		robot.TaskOnline = false
		robot.Status.TaskOnline = false
	}
	if robot.Status.Online && !robot.Status.TaskOnline && robot.Status.ControlType == 1 && robot.Account.AgvIP != "127.0.0.1" {
		//当前时间减去上一次时间
		if time.Now().Sub(robot.lastReconnectTime) > 5*time.Second {
			log.Warnf("robot:%s 是自动状态,机器人在线但是任务不在线，触发重新上线逻辑", robot.Status.AgvId)
			reconnectCallback(robot)
			robot.lastReconnectTime = time.Now()
		}

	}
}

type RobotGroupTask struct {
	GroupID         string
	CanDealRobotNum int
	TaskList        []Task
}

type RobotAccount struct {
	AgvId       string `json:"agv_id" bson:"agv_id" binding:"required"`
	AgvIP       string `json:"agv_ip" bson:"agv_ip" binding:"required"`
	AgvPort     int    `json:"agv_port" bson:"agv_port" binding:"required"`
	CategoryID  int    `json:"category_id" bson:"category_id" binding:"required"`
	MapName     string `json:"map_name" bson:"map_name"`
	GroupID     string `json:"group_id" bson:"group_id"` //机器人分组id
	AutoCharge  bool   `json:"auto_charge" bson:"auto_charge"`
	AutoPark    bool   `json:"auto_park" bson:"auto_park"`
	IsEnable    bool   `json:"is_enable" bson:"is_enable"`
	GoodsStatus int32  `json:"goods_status" bson:"goods_status"`
}

func (RobotAccount) TableName() string {
	return "t_data_robots"
}
